//
// Created by az on 2020/7/15.
//

#ifndef WMR_BEZIER_PLANNER_ATTOM_NAV_H
#define WMR_BEZIER_PLANNER_ATTOM_NAV_H

#include <eigen3/Eigen/Dense>

#include <vector>
#include <fstream>
#include <mutex>
#include <ctime>
#include <Eigen/Dense>
#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <memory>

#include "wmr_msgs/PolynomialTrajectory.h"
#include "pidcontroller.h"
#include "oriTrans.h"

//#include "opencv2/core.hpp"
//#include "opencv2/highgui.hpp"

#define D2R         (double)M_PI/180
#define VRESL       0.02
#define WRESL       2*D2R

struct EvalATTOM {
    double v, w, qe, te, de, ve;

    EvalATTOM(double _v, double _w, double _qe, double _te, double _de, double _ve) :
            v(_v), w(_w), qe(_qe), te(_te), de(_de), ve(_ve) {}
};

struct QE {
    double xe, ye, te;

    QE(double _xe, double _ye, double _te) : xe(_xe), ye(_ye), te(_te) {}
};

struct RobState{
    double x;
    double y;
    double yaw;
    double velocity;
    double angular;

    RobState(double _x, double _y, double _yaw, double _v, double _w) :
    x(_x), y(_y), yaw(_yaw), velocity(_v), angular(_w) {}
} ;

class ATTOM{
private:
    double Vmax_, Wmax_, DVmax_, DWmax_, exe_time_, pre_time_;
    double vmin_, vmax_, wmin_, wmax_;

    double q1_, q2_, q3_, e1_, e2_, e3_, e4_;
    double v_output_, w_output_, start_time_;
    double qe_sum_, te_sum_, de_sum_, ve_sum_;
    double robot_radius_, dis_retraj_;
    bool isPeriodStart_, isReTraj_, angleCtrl_, reachTarget_;

    std::vector<EvalATTOM> vec_eval_;
    std::vector<QE> vec_QE_;
    std::vector<RobState> vec_pr_;

    RobState * p_;

public:
    inline ATTOM(double Vmax, double Wmax, double DV, double DW,
                 double EXEtime, double PREtime, double radius, double retraj)
            :   vmin_(0), vmax_(0), wmin_(0), wmax_(0),
                q1_(2), q2_(2), q3_(0.5),
                e1_(1), e2_(1), e3_(0), e4_(0),
                v_output_(0), w_output_(0), start_time_(0),
                qe_sum_(0), te_sum_(0), de_sum_(0), ve_sum_(0),
                isPeriodStart_(true), isReTraj_(false), angleCtrl_(true), reachTarget_(false){
        Vmax_ = Vmax;
        Wmax_ = Wmax;
        DVmax_ = DV;
        DWmax_ = DW;
        exe_time_ = EXEtime;
        pre_time_ = PREtime;
        robot_radius_ = radius;
        dis_retraj_ = retraj;

        p_ = new RobState(0, 0, 0, 0, 0);
    }

    ~ATTOM(){
        std::vector<EvalATTOM>().swap(vec_eval_);
        std::vector<RobState>().swap(vec_pr_);
        std::vector<QE>().swap(vec_QE_);
        delete(p_);
    }

    inline void setQ(double _q1, double _q2, double _q3){
        this->q1_ = _q1;
        this->q2_ = _q2;
        this->q3_ = _q3;
    }
    void setRobotState(Eigen::Vector3d& p_3d, double& yaw, double& v, double& w);

    bool run(double& velocity, double& angular, bool is_PID_open = false);
protected:
    void calcDynamicWindow(double& vmin, double& vmax, double& wmin, double& wmax);
    void genTrajPoint(RobState& p_eval, const double& vt, const double& wt, const double& t);

    void evaluation();
    void normalize();
    void finalEvaluate();
    bool isSwitch();
    bool getPr();
    bool calcQE(const double& vt, const double& wt, double& qe, double& dis, std::vector<RobState>& traj);
    double CalcTermC(RobState& pt);
    double CalcVel(double _v);

    void writeError();
    //angle control
    void anglePIDCtrl();
    bool isFinish();
    bool isReady(bool is_PID_open);

public:
    bool anglePIDMark() {return angleCtrl_;}
    bool getReachMark() {return reachTarget_;}
    bool getReTrajMark() {return isReTraj_;}
    void setPIDMark(bool mark) {angleCtrl_ = mark;}
    inline void resetMark() {
        reachTarget_ = false;
        isReTraj_    = false;
    }
};

#endif //WMR_BEZIER_PLANNER_ATTOM_NAV_H
